# -*- coding: utf-8 -*-
'''
Created on 14.11.2019

@author: yu03
'''

import glob
import re
import datetime
import cv2
import matplotlib.pyplot as plt
from scipy import signal
from FFT_Interpolation import FFT_interpolation_2, line_cal, line_cal_fix
import numpy as np
import os
from scipy.optimize import curve_fit
from mpl_toolkits.mplot3d import Axes3D
from PyUeye_Unified.Full_Frame_Raw10 import folder_path, doc_name, np_names, hor_index, ver_lines, ver_index, hor_lines, line_num
import sys
import datetime
now = datetime.datetime.now()

def Export_Data(file_name, header, out_str):
    print('Writing Data')
    with open(file_name,'w') as fid: ######################################################################################
        fid.writelines(header)
        fid.writelines(out_str)
    print('Finish Writing')
    return

# calculation_mode = 'diff' ### DC removed
calculation_mode = 'DC' ### DC Not removed


''' 
    General Parameters
'''
fs_cam = 50
Lamda = 633e-9
pix_size = 5.3e-6
V_x, V_y, V_z = 0, 0, 0

for np_file in np_names:
    ''' 
        File name define
    '''
    file_name = re.findall(r"Resolution\\(.+).npy", np_file)[0]
#     if calculation_mode == 'DC':
#         np_result_name = folder_path + '\\results\\' + file_name + '_lines' + '.npy'
#     elif calculation_mode == 'diff':
#         np_result_name = folder_path + '\\results\\diff\\' + file_name + '_lines_diff' + '.npy'
#     else:
#         sys.exit('FFT Mode Error:\n calculation_mode = %s'%calculation_mode)
    print('File:', file_name, '; ', datetime.datetime.now().time())

    ''' 
        Reading .npy file
    '''
    f = open(np_file, 'rb')
#     print(np_file)
    file_name = np_file.split('\\')[-1]
#     print(file_name.split('.')[0])
    if calculation_mode == 'DC':
        txt_name = folder_path + '\\results\\' + file_name.split('.')[0] + '.txt'
    elif calculation_mode == 'diff':
        txt_name = folder_path + '\\results\\diff\\' + file_name.split('.')[0] + '_diff.txt'
    else:
        sys.exit('FFT Mode Error:\n calculation_mode = %s'%calculation_mode)
    f_size = os.fstat(f.fileno()).st_size
    frames = []
    time_sequence = []
    while f.tell() < f_size:
    #         print(f.tell())
        frame = np.load(f, allow_pickle=True)
        frames.append(frame[0])
        time_sequence.append(frame[1])
     
    # x_axis = np.linspace(0, len(time_sequence)-1, len(time_sequence))
     
    frames = np.array(frames)
    print(frames.shape)
    img_set = frames
    frame_total = len(frames)
    
#     im = plt.imshow(frames[0])
#     plt.colorbar(im)
#     plt.show()
    
    ''' 
        Finding Phase Spectrum Component
    '''
    hor_f_fit_set = []
    ver_f_fit_set = []
    hor_phase_centers = []
    ver_phase_centers = []
    hor_m_k_num_set = []
    ver_m_k_num_set = []
    for img in img_set[::5]:
        hor_center = np.array(img[hor_index])
        ver_center = np.array(img[:, ver_index])
#             hor_center = np.array([i[0]+256*i[1] for i in hor_center])
#             ver_center = np.array([i[0]+256*i[1] for i in ver_center])
        if calculation_mode == 'diff':
            hor_center = np.diff(hor_center.astype('int'))
            hor_center = np.concatenate(([0], hor_center))
            ver_center = np.diff(ver_center.astype('int'))
            ver_center = np.concatenate(([0], ver_center))
 
        elif calculation_mode == 'DC':
            pass
        else:
            sys.exit('FFT Mode Error:\n calculation_mode = %s'%calculation_mode)
         
        hor_f_fit, hor_phase_estim, hor_m_k_num = line_cal(hor_center)
        ver_f_fit, ver_phase_estim, ver_m_k_num = line_cal(ver_center)
         
        hor_m_k_num_set.append(hor_m_k_num)
        ver_m_k_num_set.append(ver_m_k_num)
 
    m_k_num_hor = int(np.average(hor_m_k_num_set))
    m_k_num_ver = int(np.average(ver_m_k_num_set))
    print(frame_total, "Frames;  ", 'Chosen Components:', m_k_num_hor, m_k_num_ver, '  ', datetime.datetime.now().time() )
    ''' 
        Processing
    '''   
    frame_num = 0
    line_num = 100
    for img in img_set:
#     for img in img_set[:10]:
        frame_num += 1
        hor_phases, ver_phases = [], []
        hor_freqs, ver_freqs = [], []
        for i in range(line_num):
            hor_center = np.array(img[hor_lines[i]])
            ver_center = img[:, ver_lines[i]]
#                 hor_center = np.array([i[0]+256*i[1] for i in hor_center])
#                 ver_center = np.array([i[0]+256*i[1] for i in ver_center])
             
            if calculation_mode == 'diff':
                hor_center = np.diff(hor_center.astype('int'))
                hor_center = np.concatenate(([0], hor_center))
                ver_center = np.diff(ver_center.astype('int'))
                ver_center = np.concatenate(([0], ver_center))
     
            elif calculation_mode == 'DC':
                pass
            else:
                sys.exit('FFT Mode Error:\n calculation_mode = %s'%calculation_mode)
             
            hor_f_fit, hor_phase_estim, hor_m_k_num = line_cal_fix(hor_center, m_k_num_hor, m_k_num_ver)
            ver_f_fit, ver_phase_estim, ver_m_k_num = line_cal_fix(ver_center, m_k_num_hor, m_k_num_ver)
#             print(hor_f_fit, ver_f_fit)
             
            hor_freqs.append(hor_f_fit)
            hor_phases.append(hor_phase_estim)
            ver_freqs.append(ver_f_fit)
            ver_phases.append(ver_phase_estim)
             
        hor_phases = np.array(hor_phases)
        hor_freqs = np.array(hor_freqs)
        ver_phases = np.array(ver_phases)
        ver_freqs = np.array(ver_freqs)            
            
        hor_f_fit_set.append(np.sum(hor_freqs)/line_num)
        ver_f_fit_set.append(np.sum(ver_freqs)/line_num)
         
        fitted_phase = np.sum(np.unwrap(hor_phases))/line_num
        if fitted_phase > np.pi:
            fitted_phase = fitted_phase - 2*np.pi
        if fitted_phase < -np.pi:
            fitted_phase = fitted_phase + 2*np.pi
        hor_phase_centers.append(fitted_phase)
         
        fitted_phase = np.sum(np.unwrap(ver_phases))/line_num
        if fitted_phase > np.pi:
            fitted_phase = fitted_phase - 2*np.pi
        if fitted_phase < -np.pi:
            fitted_phase = fitted_phase + 2*np.pi
        ver_phase_centers.append(fitted_phase)
        if frame_num%10 == 0:
            print('Frame: ', frame_num, '   ',datetime.datetime.now().time())   
         
     
    hor_f_fit_set = np.array(hor_f_fit_set)
    ver_f_fit_set = np.array(ver_f_fit_set)
    hor_phase_centers = np.array(hor_phase_centers)
    ver_phase_centers = np.array(ver_phase_centers)
     
    hor_angle = (V_x-Lamda*hor_f_fit_set/2)*1e6 ### urad
    hor_length = np.unwrap(hor_phase_centers)/4/np.pi*Lamda*1e9 ### nm
    ver_angle = (V_y-Lamda*ver_f_fit_set/2)*1e6 ### urad
    ver_length = np.unwrap(ver_phase_centers)/4/np.pi*Lamda*1e9 ### nm
         
    ''' 
        Exporting
    '''
     
    header = ['%s\n' %doc_name,
          'Local current time : %s\n' %now.strftime("%Y-%m-%d %H:%M:%S"),
          'Fs = %e (Hz)\n' %fs_cam,##########################################################################################################
          'Record time: %e (s)\n' %(len(frames)/fs_cam),############################################################################################
          'Frame Number = %i\n' %len(frames),############################################################################################
          'Lines/Frame = %i\n' %len(frames[0]),############################################################################################
          'Pixels/Line = %i\n' %len(frames[0][0]),############################################################################################
          'Channel_1: Horizontal freq.(101-line averaged)\n',############################################################################################
          'Channel_2: Horizontal phase(101-line averaged)\n',############################################################################################
          'Channel_3: Vertical freq.(101-line averaged)\n',############################################################################################
          'Channel_4: Vertical phase(101-line averaged)\n',############################################################################################
    #       'Channel_3: Demodulated S_curve (from CH_No.2)\n',############################################################################################
    #       'Channel_4: Demodulated S_curve (from CH_No.1)\n',############################################################################################
    #       'Data_Ch5: Difference between Channel_3 & Channel_4 (Ch3-Ch4)\n'################################################################################
          '-------------------------------------------------\n',
          ]
    out_str = ['%f, %f, %f, %f\n' %(hor_f_fit_set[i], hor_phase_centers[i], ver_f_fit_set[i], ver_phase_centers[i]) for i in range(len(hor_f_fit_set))]     
     
    ''' 
        Saving
    '''
    Export_Data(txt_name, header, out_str)
    print('TXT file saved')
     
if 1:### Plotting Noise_PSD
    ############### linearity range
    start_cutting, stop_cutting = 0, len(hor_angle)
#     start_cutting, stop_cutting = 20, 3000
    selected_range = np.linspace(start_cutting, stop_cutting, stop_cutting-start_cutting) 
    ############### Plotting
      
    plt.figure('Tilt')
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
         
    ax2 = plt.subplot(3,2,2)
    range = [0, len(ver_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_angle
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Tilting")
    plt.ylabel("Vertical Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax3 = plt.subplot(3,2,3)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_angle-np.average(hor_angle))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax4 = plt.subplot(3,2,4)
    range = [0, len(ver_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (ver_angle-np.average(ver_angle))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Tilting")
    plt.ylabel("Vertical Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
 
    ax5 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_angle, fs_cam, nperseg=len(hor_angle))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Tilting Noise_PSD")
    plt.ylabel("PSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-6, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    ax6 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(ver_angle, fs_cam, nperseg=len(ver_angle))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Vertical Tilting Noise_PSD")
    plt.ylabel("PSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-6, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    plt.tight_layout()
     
    plt.figure('Length')
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
         
    ax2 = plt.subplot(3,2,2)
    range = [0, len(ver_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_length
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Length")
    plt.ylabel("Vertical Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax3 = plt.subplot(3,2,3)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (hor_length-np.average(hor_length))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax4 = plt.subplot(3,2,4)
    range = [0, len(ver_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = (ver_length-np.average(ver_length))[range[0]:range[1]]
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Length")
    plt.ylabel("Vertical Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
 
    ax5 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_length, fs_cam, nperseg=len(hor_length))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Length Noise_PSD")
    plt.ylabel("PSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-6, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    ax6 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(ver_length, fs_cam, nperseg=len(ver_length))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Vertical Length Noise_PSD")
    plt.ylabel("PSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-6, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    plt.tight_layout()
     
    plt.show()
 
     
if 0:### Plotting Nonlinearity
    ############### linearity range
    start_cutting, stop_cutting = 0, len(hor_phase_centers)
    start_cutting, stop_cutting = start_cutting/fs_cam, stop_cutting/fs_cam
    zoom_time = start_cutting, stop_cutting
     
    start_cutting, stop_cutting = 1.6, 6.2 ### in second   
#     zoom_time = [1.2, 1.4]
     
    start_cutting, stop_cutting = int(start_cutting*fs_cam), int(stop_cutting*fs_cam)
    selected_range = np.linspace(start_cutting, stop_cutting, stop_cutting-start_cutting)
    selected_time = (np.arange(stop_cutting-start_cutting)+start_cutting)/fs_cam    
    zoom_range = [zoom_time[0]*fs_cam, zoom_time[1]*fs_cam]
      
    plt.figure('Tilt')
    plt.gcf().set_size_inches(18,9)
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle
    plt.plot(x, y, color='blue', marker=' ')
     
    measured_angle = hor_angle[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_angle)-1, len(measured_angle)), measured_angle, 1)
    linear_fit = np.linspace(0, len(measured_angle)-1, len(measured_angle))*p + q
    hor_angle_nonlinearity = (measured_angle - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
         
    ax2 = plt.subplot(3,2,2)
    range = [0, len(ver_angle)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_angle
    plt.plot(x, y, color='blue', marker=' ')
     
    measured_angle = ver_angle[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_angle)-1, len(measured_angle)), measured_angle, 1)
    linear_fit = np.linspace(0, len(measured_angle)-1, len(measured_angle))*p + q
    ver_angle_nonlinearity = (measured_angle - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Vertical Tilting")
    plt.ylabel("Vertical Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax3 = plt.subplot(3,2,3)
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_angle_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Tilting")
    plt.ylabel("Horizontal Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax4 = plt.subplot(3,2,4)
    plt.plot(x, y, color='red', marker=' ')
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_angle_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Tilting")
    plt.ylabel("Vertical Angle (urad)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
 
#     ax5 = plt.subplot(4,2,5)
#     range = [int(zoom_range[0]-start_cutting), int(zoom_range[1]-start_cutting)]
#     x = (np.arange(zoom_range[1]-zoom_range[0])+zoom_range[0])/fs_cam
#     y = hor_angle_nonlinearity[range[0]:range[1]]
#     plt.plot(x, y, color='blue', marker=' ')
#     plt.title("Horizontal Tilting")
#     plt.ylabel("Horizontal Angle (urad)")
#     plt.xlabel("Time (s)")
#     plt.grid(which='both', axis='both')
#        
#     ax6 = plt.subplot(4,2,6)
#     range = [int(zoom_range[0]-start_cutting), int(zoom_range[1]-start_cutting)]
#     x = (np.arange(zoom_range[1]-zoom_range[0])+zoom_range[0])/fs_cam
#     y = ver_angle_nonlinearity[range[0]:range[1]]
#     plt.plot(x, y, color='blue', marker=' ')
#     plt.title("Vertical Tilting")
#     plt.ylabel("Vertical Angle (urad)")
#     plt.xlabel("Time (s)")
#     plt.grid(which='both', axis='both')
 
    ax7 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_angle_nonlinearity, fs_cam, nperseg=len(hor_angle_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Tilting Noise_PSD")
    plt.ylabel("PSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-4, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    ax8 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(ver_angle_nonlinearity, fs_cam, nperseg=len(ver_angle_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Vertical Tilting Noise_PSD")
    plt.ylabel("PSD (urad/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-4, 10) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    plt.tight_layout()
     
     
    #### Length
    plt.figure('Length')
    plt.gcf().set_size_inches(18,9)
     
    ax1 = plt.subplot(3,2,1)
    range = [0, len(hor_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length
    plt.plot(x, y, color='blue', marker=' ')
     
    measured_length = hor_length[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_length)-1, len(measured_length)), measured_length, 1)
    linear_fit = np.linspace(0, len(measured_length)-1, len(measured_length))*p + q
    hor_length_nonlinearity = (measured_length - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
         
    ax2 = plt.subplot(3,2,2)
    range = [0, len(ver_length)]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_length
    plt.plot(x, y, color='blue', marker=' ')
     
    measured_length = ver_length[start_cutting:stop_cutting]
    [p, q] = np.polyfit(np.linspace(0, len(measured_length)-1, len(measured_length)), measured_length, 1)
    linear_fit = np.linspace(0, len(measured_length)-1, len(measured_length))*p + q
    ver_length_nonlinearity = (measured_length - linear_fit)
    plt.plot(selected_time, linear_fit, color='red')
    plt.title("Vertical Length")
    plt.ylabel("Vertical Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
       
    ax3 = plt.subplot(3,2,3)
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = hor_length_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Horizontal Length")
    plt.ylabel("Horizontal Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
        
    ax4 = plt.subplot(3,2,4)
    plt.plot(x, y, color='red', marker=' ')
    range = [start_cutting, stop_cutting]
    x = (np.arange(range[1]-range[0])+range[0])/fs_cam
    y = ver_length_nonlinearity
    plt.plot(x, y, color='blue', marker=' ')
    plt.title("Vertical Length")
    plt.ylabel("Vertical Length (nm)")
    plt.xlabel("Time (s)")
    plt.grid(which='both', axis='both')
     
#     ax5 = plt.subplot(4,2,5)
#     range = [int(zoom_range[0]-start_cutting), int(zoom_range[1]-start_cutting)]
#     x = (np.arange(zoom_range[1]-zoom_range[0])+zoom_range[0])/fs_cam
#     y = hor_length_nonlinearity[range[0]:range[1]]
#     plt.plot(x, y, color='blue', marker=' ')
#     plt.title("Horizontal Length")
#     plt.ylabel("Horizontal Length (nm)")
#     plt.xlabel("Time (s)")
#     plt.grid(which='both', axis='both')
#        
#     ax6 = plt.subplot(4,2,6)
#     range = [int(zoom_range[0]-start_cutting), int(zoom_range[1]-start_cutting)]
#     x = (np.arange(zoom_range[1]-zoom_range[0])+zoom_range[0])/fs_cam
#     y = ver_length_nonlinearity[range[0]:range[1]]
#     plt.plot(x, y, color='blue', marker=' ')
#     plt.title("Vertical Length")
#     plt.ylabel("Vertical Length (nm)")
#     plt.xlabel("Time (s)")
#     plt.grid(which='both', axis='both')
 
    ax7 = plt.subplot(3,2,5)
    f_line, PS = signal.welch(hor_length_nonlinearity, fs_cam, nperseg=len(hor_length_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Horizontal Length Noise_PSD")
    plt.ylabel("PSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-3, 2e3) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    ax8 = plt.subplot(3,2,6)
    f_line, PS = signal.welch(ver_length_nonlinearity, fs_cam, nperseg=len(ver_length_nonlinearity))
    PSD = np.sqrt(PS)
    plt.loglog(f_line, PSD, 'b')
    plt.title("Vertical Length Noise_PSD")
    plt.ylabel("PSD (nm/$\sqrt{Hz}$)")
    plt.xlabel("Freq. (Hz)")
    plt.grid(which='both', axis='both')
    plt.ylim(1e-3, 2e3) # for Ramp/20s noise
#     plt.ylim(1e-4, 0.1) # for noise
     
    plt.tight_layout()
    plt.show()
            
            
            
